/////////////////////////////////////////////////////////////////////////////
//
// The Neurosciences Institute, 2011
//
// Project:
//      Conscious Artifact
//
// Author:
//       Rich Martin, et al
//
// Description:
//       world class encapsulates the world.
//       world::interact() allows you to ineract with the robot/environment.
//       Ideally, all communicating with the robot would be hidden inside this object
//       s.t. if a particular processor needs to get/set some value, it will be 
//       available from this object, even if from a romote host.
//
//       For now, we can have each node simulate an arm redundantly.
//
/////////////////////////////////////////////////////////////////////////////

#include <iostream>
#include <iomanip>
#include <fstream>
#include <string>
#include <sstream>
#include <algorithm>
#include <vector>
#include <map>
#include <cmath>
#include <limits.h>
#include <stdint.h>
#include <mpi.h>
#include "motormap.h"
#include "groups.h"
#include "main.h"

using namespace std;

#ifdef SIMULATED_ROBOT
#define APEX2
#else
#define APEX3
#endif // SIMULATED_ROBOT


#ifdef APEX1
#include "robot_real.h"
#endif

#ifdef APEX2
#include "robot_sim.h"
#endif

#ifdef APEX3
#include "apex_robot.h"
#endif

/**************************************/
// Experiment phases
// #define PHASE_1_2_ // V1 and P pattern training
// #define PHASE_3_ // M1 motor response training
// #define PHASE_4_ // Full control DMS test
// #define PHASE_5_ // V1 and P sequence training
#define PHASE_6_ // Full DMS test

#include "generator.h"
UnifRandGen my_generator = UnifRandGen(RAND_SEED);

extern float x[][3];

extern float DA; //dopamine excitation
extern float DA_decay_rate;
extern float sd_decay_rate;
extern int APEX_REWARD;
bool reward_button_pressed = 0;

/****** RGM: Expt. time settings ******/	
int ms_per_sec = 1000;												

//Expt. time settings	
int start_no_reset_training_sec = 160;//20=>quick tests;//originally 80 for hierarchical model
int first_seq_stim_start_sec = 40;	
int testing_start_time_sec = 590;//270;//+320;
int training_end_time_sec = testing_start_time_sec-1;																
int M1_stim_end_time_sec = 590+320; //1000;//continue stimulation ad infinitum
int V1_stim_start_time_sec = 1000;//1000
int V1_stim_end_time_sec = 1000;//testing_start_time_sec;//32;
int training_sample_duration_ms = 1000;
int testing_sample_duration_ms = 1000;

#ifdef SIMULATED_ROBOT
bool kill_sim_at_specified_time = true;//turn to false after training is over
#else
bool kill_sim_at_specified_time = true;
#endif

#ifdef PHASE_1_2_
#define start_dynamics
#define Save_Dynamics_Every_Second
int kill_sim_time_sec = 169; //testing_start_time_sec + 90;//testing_start_time_sec; //32; //end of preliminary training
#endif
#ifdef PHASE_3_
#define Save_Dynamics_Every_Second
int kill_sim_time_sec = 490; //testing_start_time_sec + 90;//testing_start_time_sec; //32; //end of preliminary training
#endif
#ifdef PHASE_4_
#define Save_Dynamics_Every_Second
int kill_sim_time_sec = 810; //testing_start_time_sec + 90;//testing_start_time_sec; //32; //end of preliminary training
#endif
#ifdef PHASE_5_
#define Save_Dynamics_Every_Second
int kill_sim_time_sec = 905; //testing_start_time_sec + 90;//testing_start_time_sec; //32; //end of preliminary training
#endif
#ifdef PHASE_6_
#define Save_Dynamics_Every_Second
int kill_sim_time_sec = 1225; //testing_start_time_sec + 90;//testing_start_time_sec; //32; //end of preliminary training
#endif

int reset_window_training = training_sample_duration_ms;
int reset_offset_training_A = 50; //training_sample_duration_ms - 1
int reset_offset_training_B = 100; //training_sample_duration_ms - 1; //50
int reset_offset_training_C = 0;// 100, 0
int reset_offset_training_D = 10;// 100, 0

int reset_window_testing = testing_sample_duration_ms;
int reset_offset_testing_A = 50; //testing_sample_duration_ms - 1; 
int reset_offset_testing_B = 100;//testing_sample_duration_ms - 1; //50
int reset_offset_testing_C = 0;//0
int reset_offset_testing_D = 10;//0

/* Rotation Station Definitions */
unsigned int rot_cntr = 0;
//float pose_matrix[] = { 0, 45, 90, 135 };//0, 90, 180, 270
float pose_matrix[] = { 0, 90, 180, 270 };
bool wheel = false; // false;

/* Robot Mental Rotation Parameters */
int pattern_A = 0;//global
int pattern_A_sec = 0;//
int pattern_B = 0;//global
int pattern_B_sec = 1500;
int rotate_head_sec_a = 1000;
int rotate_head_sec_b = 3000; 
int current_pattern = 0;
int trial_counter = -1;
int record_counter = 0;

// ********************************************************************* 
int start_time_ms = 170000;//testing_start_time_sec*ms_per_sec;
int pattern_trial_duration = 5000;//1000;//5000;
int lookat_A_ms = 0;
int lookaway_A_ms = 1000;//900;//1000;
int lookat_B_ms = 2000;
int lookaway_B_ms = 3000;
bool match_flag = false;
bool first_match = false;
// ********************************************************************* 
int min_pattern = 0;
int max_pattern = 3;
int action_pattern = 0;
bool motor_flag = false;
unsigned int head_pose = 0;
unsigned int head_pose_home = 443;
unsigned int head_pose_rotate = 275;//275;//right lower, left higher
unsigned int head_speed = 800;

//float max_degrees = 300.0;
//unsigned int resolution = 1023;   
//unsigned int pos_raw = (unsigned int)( (pos_degrees / max_degrees) * resolution);
//unsigned int rot_sta_poses[] = { 0, 306, 613, 920 };
unsigned int rot_sta_poses[] = { 0, 306, 613, 920, 0, 306, 613, 920 };
unsigned int blinder_up  = 360;
unsigned int blinder_down = 30;

unsigned int BLINDER_ID = 18;
unsigned int ROTATION_ID = 19;
unsigned int SPINNER_ID = 20;
unsigned int spinner_object1 = 20;
unsigned int spinner_object2 = 645;

ofstream DA_debugger;
ofstream button_timings;

//orignal home positions float home_pose_radians[] = { 0.851359, 1.08146, 0.32725 }; //shoulder, deltoid, elbow
float home_pose_radians[] = { 1.0, 1.2, 1.5 }; //shoulder, deltoid, elbow
float target_pose_radians[] = { 0.102142, 0.406207, 0.611036 }; //shoulder, deltoid, elbow
float start_pose = 0.0;
float current_pose = 0.0;
float target_threshold = 0.05; //radians
float movement_threshold = 0.25; //radians
float M1_stim_current = 0;
bool arm_moved_to_target = false;
bool arm_moved = false;
bool MTS_active = false;
bool coin_flip = false;
bool is_match_trial = false;
int conditioning_cnt = 0;

class electrode
{
public:
  float position[3];
  int start_msec;
  int stop_msec;
  float dist;
  float stim_current;
};

//*******************************************
float dist(float x[3], float y[3], int n=3)
{
  float d = 0.0f;

  for(int i = 0; i < n; i++)
  {
    d+=(x[i]-y[i])*(x[i]-y[i]);
  }

  return sqrt(d);
}

//world clas wrapper for interact functions, etc.
class world 
{
private:
  vector<float> mfr;
  vector<electrode> electrodes;

public:
#ifdef APEX3
  typedef ::nsi::thinklink::scoped_ptr< ::nsi::thinklink::device > device;
#endif

  void init(group igroup_data[MAX_AREAS][MAX_TYPES]) 
  {
    sim_end_time_sec = 17000;

#ifdef APEX3

#if defined(PHASE_1_2_) || defined(PHASE_5_)
    /* Pattern + Rotation Training Settings */
    trial_start_ms = 0;//start_time_ms;
    trial_duration = 1000;//pattern_trial_duration;
    lookat_A = 0;//lookat_A_ms;
    lookaway_A = 900;//lookaway_A_ms;
    lookat_B = 1000;//lookat_B_ms;
    lookaway_B = 1000;//lookaway_B_ms;
#endif
#if defined(PHASE_3_) || defined(PHASE_4_) || defined(PHASE_6_)
    /* Blank-Out Test Settings */    
    trial_start_ms = start_time_ms;
    trial_duration = pattern_trial_duration;
    lookat_A = lookat_A_ms;
    lookaway_A = lookaway_A_ms;
    lookat_B = lookat_B_ms;
    lookaway_B = lookaway_B_ms;
#endif
    

#endif

    // initialization ...
    group_data.load_groups(igroup_data);

    max_neuron_id = group_data.get_max_neuron_id();

    mfr.resize(max_neuron_id+1,0.0);

    //*************************************
    // Set up the motor control system.
    // Assume that each spot in the layer V pyramidal cell array
    // has a specific muscle synergy that is position based.
    // 
    group g = group_data["M1p5(L56)"];

    vector<float> one_row;
    one_row.resize(g.cols,0.0f);

    shoulder_lut_rad.resize(g.rows,one_row);
    deltoid_lut_rad.resize(g.rows,one_row);
    elbow_lut_rad.resize(g.rows,one_row);

    float prow = 0.0f;
    for( int row = 0; row< g.rows; row++)
    {
      float pcol = 0.0f;
      for( int col = 0; col < g.cols; col++ )
      {
        shoulder_lut_rad[row][col] = motorprimitives[(int) prow*25+(int)pcol][0] * range_rad[0];
        deltoid_lut_rad[row][col]  = 1.0 * range_rad[2];//motorprimitives[(int) prow*25+(int)pcol][2] * range_rad[1];//RGM DEBUG 08/08/2012 [2];
        elbow_lut_rad[row][col]    = motorprimitives[(int) prow*25+(int)pcol][1] * range_rad[2];//RGM DEBUG 08/08/2012 [1];		
        pcol=pcol+24.999/g.cols;
      }
      prow=prow+24.999/g.rows;
    }
    fprintf(stderr, "Finished loading world\n");  
  }

  ~world() 
  {
    cout << "[WORLD DESTROYER] Quitting now. Goodbye.\n" << flush;

    if( myprocid == 0 )
    {
      fout.close();
      patternhist.close();
    }
  }

#ifdef APEX3
  void reset()
  {
    apex_client.reset_link();
  }	
#endif

  void set_procid(int myid)
  {		
    myprocid = myid;
  }		  


#ifdef APEX3		
  void init_robot(int myid, ::nsi::thinklink::device* link)
  {
#else
  void init_robot(int myid)
  {		
#endif

    myprocid = myid;

#ifdef APEX3
    if( myprocid == 0 )
    {
      //initialize robot
      std::string gait_file = "LV_New_Walk_Concat4.bin";
      apex_client.init(gait_file, link);
    }
#else
    apex.init(myid);
#endif
    if( myprocid == 0 )
    {
      fout.open("motorcommands.dat");
      patternhist.open("patternhist.dat");
      DA_debugger.open("DA_debugger.dat");
      button_timings.open("button_timings.dat");
    }
  }

  // It returns a random permutation of 0..n-1
  int** rand_perm2D(int mmm, int nnn) {
    std::cerr << "Going in RAND_PERM2D: " << myprocid << std::endl;
    ofstream ftemp;
    if( myprocid == 0 ) {
      ftemp.open("randperms.dat");
    }
    int **toReturn = (int**)malloc(mmm*sizeof(int*));
    int l;
    for (l = 0; l < mmm; l++) {
      toReturn[l] = (int*)malloc(nnn*sizeof(int));
      int k;
      for (k = 0; k < nnn; k++) {
        toReturn[l][k] = k;
      }
      for (k = nnn-1; k > 0; k--) {
        int j = my_generator.UrandI(0,INT_MAX) % (k+1);
        int temp = toReturn[l][j];
        toReturn[l][j] = toReturn[l][k];
        toReturn[l][k] = temp;
      }
      if (myprocid == 0) {
        for (k = 0; k < nnn; k++) {
          ftemp << toReturn[l][k] << " ";
        }
        ftemp << endl;
      }
    }
    if( myprocid == 0 ) {
      ftemp.close();
    }
    std::cerr << "Going out RAND_PERM2D: " << myprocid << std::endl;
    return toReturn;
  }

  int myrand(int low, int hi)
  {
    return low + rand()%hi;	
  }

  float to01(float x,float minx,float maxx)
  {		
    return (x- minx)/(maxx-minx);
  } 

  // Returns muscle length as a function of joint angle in radians, given the 
  // length of the tendon insertion points from the joint.
  float length(float a, float b, float theta_rad)
  {
    return sqrt(a*a+b*b-2*a*b*cos(theta_rad));
  }

  // Returns derivative of the muscle length with respect to the joint angle (radians),
  // given the length of the tendon insertion points from the joint.
  // This can be used to convert angular joint velocity in radians per second to muscle 
  // length changes in linear units by calculating dtheta*dlength(theta).
  float dlength(float a, float b, float theta_rad)
  {
    return a*b*sin(theta_rad)/sqrt(a*a+b*b-2*a*b*cos(theta_rad));
  }	

  //***************************************************************************************************
  void stim_group(string group_name, vector<electrode> electrodes, int t_msec, int trial_duration_msec, float exc_output[])
  {
    group g;
    if( group_data.exists(group_name) )
    {
      g = group_data[group_name];
      int gnum=g.last_neuron_id-g.first_neuron_id;
      int i = g.first_neuron_id;
      for( int row = 0; row< g.rows; row++)
      {
        for( int col = 0; col < g.cols; col++ )
        {
          for( int elect=0; elect < electrodes.size(); elect ++ )
          {
            if( t_msec%trial_duration_msec >= electrodes[elect].start_msec && t_msec%trial_duration_msec <= electrodes[elect].stop_msec)
            {
              if( dist(x[i], electrodes[elect].position) < electrodes[elect].dist )
              {
                exc_output[i] = electrodes[elect].stim_current;
              }
            }			
          }							
          i++;
        }
      }
    }
  }		

  //***************************************************************************************************
  // Assumptions: outarray is a a 1d array pixels by pixels long that holds the gabor function output.
  // Sx and Sy is the center of the gabor; theta is the angle, cycles is the number of cycles; all units in radians
  // phase_percent is 0 to 1 and shifts the phase.
  void gabor(float* outarray, int pixels,float Sx,float Sy,float theta, float cycles, float phase_percent, float scale)
  {
    float xphase = phase_percent*cycles*pi*cos(theta);
    float yphase = phase_percent*cycles*pi*sin(theta);

    float step = 2*pi/((pixels-1)/cycles);

    int c=0;
    for (float x = (-cycles*pi+xphase);x<=(cycles*pi+xphase);x+=step)
    {
      int r=0;
      for (float y = (-cycles*pi+yphase);y<=(cycles*pi+yphase);y+=step)
      {			
        float xPrime = x * cos(theta) + y * sin(theta);
        float yPrime = y * cos(theta) - x * sin(theta);
        outarray[c+r*pixels] = scale*exp(-0.5*((xPrime/Sx)*(xPrime/Sx)+(yPrime/Sy)*(yPrime/Sy)))*cos(xPrime);
        r=r+1;
      }
      c=c+1;
    }
  }

  //This function allows you to reset neural areas depending on events in the task or environment.  This sample function demonstrates several methods for doing so.  Obviously this function could get every bit as complex as interact()	  
  void reset_interact(int t) 
  {  
    /* KILL THE SIM HERE */ 
    if( kill_sim_at_specified_time )
    {
      if( t >= kill_sim_time_sec*ms_per_sec )
      {
        exit(0);
      }
    }

    //Phases 1 & 2: J Pattern Training
    if( (t < 170000) && (t%training_sample_duration_ms == 0) )	
    {
      reset_dynamics(); 
    }
                  
    //Phase 3: Conditioning
    if( (t >= 170000 && t < 490000) && (t%pattern_trial_duration == 0) )
    {
      reset_dynamics();
    }

    //Clear MTS because it might have made a match during the earlier part of the trial
    if( (t >= 170000 && t < 490000) && (t%pattern_trial_duration == 2800) )
    {
      reset_dynamics(group_data["MTSp23M"]);
      reset_dynamics(group_data["MTSb23"]);   
      reset_dynamics(group_data["M1p5(L56)"]);
      reset_dynamics(group_data["M1b5"]);
    }

    //Phase 4: Full Control Test
    if( (t >= 490000 && t < 810000) && (t%pattern_trial_duration == 0) )
    {
      reset_dynamics();
    }

    //Clear MTS because it might have made a match during the earlier part of the trial
    if( (t >= 490000 && t < 810000) && (t%pattern_trial_duration == 2800) )
    {
      reset_dynamics(group_data["MTSp23M"]);
      reset_dynamics(group_data["MTSb23"]);   
      reset_dynamics(group_data["M1p5(L56)"]);
      reset_dynamics(group_data["M1b5"]);
    }

    //PHASE 5: Sequence Training
    if( (t >= 810000 && t <= 904000) && (t%training_sample_duration_ms == 0) )
    {
      reset_dynamics(group_data["V1p23"]);
      reset_dynamics(group_data["V1b23"]); 
    }

    //silent period sequence reset
    if(t == 810000 || t == 856000 || t == 902000){
      reset_dynamics();
    }

    //Phase 6: Full Match/Non-Match Test
    if( (t >= 905000 && t < 1225000) && (t%pattern_trial_duration == 0) )
    {
      reset_dynamics();
    }

    //Clear MTS because it might have made a match during the earlier part of the trial
    if( (t >= 905000 && t < 1225000) && (t%pattern_trial_duration == 2800) )
    {
      reset_dynamics(group_data["MTSp23M"]);
      reset_dynamics(group_data["MTSb23"]);   
      reset_dynamics(group_data["M1p5(L56)"]);
      reset_dynamics(group_data["M1b5"]);
    }
  }

  //*******************************	
  // Get the spiked array so we can keep track of spikes. 
  // Get a pointer to the external current array array so we can stimulate neurons.
  // set exc_output[neuron] to add exc_output[neuron] to a given neuron number.  (Don't worry if the neuron
  // is on this processor or not; we will take care of that in the simulator.)
  // int t in msec.
  // 
  void interact( int t, bool spiked[], float exc_output[], float m1_input[], float s1_input[] )
  {
    static bool first_call = true;
    static float last_shoulder = 0,last_deltoid = 0,last_elbow = 0, last_sdot = 0, last_ddot = 0, last_edot = 0; 
    float shoulder = 0,deltoid = 0,elbow = 0, sdot = 0,ddot = 0,edot = 0, storque = 0, dtorque = 0, etorque = 0;
    bool go_home_flag = true;
    int ms_per_sec = 1000;
    int sec = t/ms_per_sec;
    int gnum,pnum;
    int kk,jj;
    group MTSp23M_group;

    int n_patterns = 8;
    
    static int loop_count = 0;
    static int **perm_vector = NULL;
    if (first_call) {
      perm_vector = rand_perm2D(n_patterns*2,4);
    }

    //first one is winner
    float xo[] = { 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2 };
    float yo[] = { 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2 };

    int trial_duration_msec = 0;
    trial_duration_msec = 1*ms_per_sec*n_patterns;

    if( sec <= training_end_time_sec ) // training phase
    {
      trial_duration_msec = training_sample_duration_ms*n_patterns;
    } 
    else // testing phase
    {
      trial_duration_msec = testing_sample_duration_ms*n_patterns;
    }

    int pattern_duration_msec = trial_duration_msec/n_patterns;

    //calculate DA_decay_rate
    float DA_tau = 500;//ms
    float sd_tau = 1000;//250;//ms
    float tau_window = 50;//ms
    float FV = 0.001;
    float PV = 1.0;
    if( t >= 170000 && t < 490000 ) // testing and conditioning
    {
      sd_tau = 1000;//ms
      DA_tau = 2000;//ms
      DA_decay_rate = PV*pow(FV, (1/(DA_tau/tau_window)));
      sd_decay_rate = PV*pow(FV, (1/(sd_tau/tau_window)));
    }		
    else // training, originally 270000
    {
      sd_tau = 6000;
      DA_tau = 1;
      DA_decay_rate = PV*pow(FV, (1/(DA_tau/tau_window)));
      sd_decay_rate = 0.951;
    }

    //Quick Pick 5 Pattern Test
    float stim_coords[][2] = { xo[12], yo[12], xo[1],  yo[1], xo[3],  yo[3], xo[9], yo[9] };

    //1) S1 Stimulation Time Parameters
    int S1_stimulation_start_time_sec = sim_end_time_sec;//0;
    int S1_stimulation_end_time_sec = sim_end_time_sec;   

    //1B)
    int S1_stimulation_start_time_sec_B = M1_stim_end_time_sec;// + 50 ;
    int S1_stimulation_end_time_sec_B = M1_stim_end_time_sec;// + 100;    

    //2) M1 Stimulation Time Parameters
    int M1_stimulation_start_time_sec = 0;//M1_stim_end_time_sec;//0;// Next, turn off layer 2/3 stimulation and see if reentry from S1 can activate layer 2.
    int M1_stimulation_end_time_sec = M1_stim_end_time_sec;//13000; 

    //3) V1 Stimulation Time Parameters
    int V1_stimulation_start_time_sec = V1_stim_start_time_sec;
    int V1_stimulation_end_time_sec = V1_stim_end_time_sec;// change back to sim_end_time_sec before checkin for the real robot

    //4) M1 Out Stimulation Time Parameters
    int M1_out_stimulation_start_time_sec = 14000+100;// sim_end_time_sec;
    //int M1_out_stimulation_start_time_sec=1300000;
    int M1_out_stimulation_end_time_sec = sim_end_time_sec-0; // First, turn off motor layer stimulation and see if layer 2/3 still activates layer 5.

    // output pattern data
    if( t%pattern_duration_msec == 0 )
    {
      patternhist << t << " " << (t%trial_duration_msec) / pattern_duration_msec << endl << flush;
    }

    int steps=11;
    int n_electrodes = steps*2*4;

    if( sec >= 0 )
    {				
#define ROBOT_TESTING_PHASE
#ifdef ROBOT_TESTING_PHASE
      if( myprocid == 0 )
      {
        if( t%pattern_trial_duration >= 3000 )
        {
          //need to latch until end of current trial
          if( APEX_REWARD == 1 )
          {
            std::cout << "WORLD: REWARD REGISTERED! " << std::endl;;
            reward_button_pressed = true;
          }
        }
        else
        {
          reward_button_pressed = false;
        }

        if( t%25 == 0 )
        {
          button_timings << t << " " << reward_button_pressed << endl << flush;
        }
      }
#endif

#define DA_REWARD_ALL_TRIALS
#ifdef DA_REWARD_ALL_TRIALS
      if( t >= 170000 && t < 490000 )
      {
        if (myprocid != 0 && t%5000 == 0) {
          conditioning_cnt++;
        }
        
        /* I expect a MATCH within the 3000 to 4999th milli-second of every 5 second trial */
        if( t%pattern_trial_duration >= 3000 )
        {
          //need to latch until end of current trial
          if( APEX_REWARD == 1 )
          {
            std::cout << "WORLD: REWARD REGISTERED! " << std::endl;;
            reward_button_pressed = true;
          }
        }

        /* Update Every 50 milli-seconds */
        if( t%50 == 0 )
        {						
          /* I expect a MATCH within the 3000 to 4999th milli-second of every 5 second trial */
          if( t%pattern_trial_duration >= 3000 )
          {
            /* Flip Coin Once At Beginning of Phase */
            if( t%pattern_trial_duration == 3000 )
            {
              /* myrand will return 1 or 2 */
              //int coin = myrand(1,2);
              
              int tempi1 = (conditioning_cnt-1) % (n_patterns*2);
              int tempi2 = (conditioning_cnt-1)/(n_patterns*2);
              int coin = perm_vector[tempi1][tempi2];
              std::cerr << "trial = " << tempi1 << ", loop = " << tempi2 << ", coin = " << coin << std::endl;

              if( coin < 2 ) {
                coin_flip = true;
              } else {
                coin_flip = false;
              }
            }
                  
            /* Shape Movement w/ M1 stimulation 50%
             * of the time, only if M1 is on (or MTS
             * is indirectly on). */
            if(coin_flip)//only coin flip, arm_moved && 
            {
              M1_stim_current = 1800;
            }
            else
            {			
              M1_stim_current = 0;
            }

            /* Reward APE-X only when he moves his 
             * arm correctly and when thinks he has
             * a match */
            if( reward_button_pressed && MTS_active ) //RGM DEBUG: realized training durring non-match trials will unlearn match conditioned responses, added MTS_active flag to prevent this
            {
              DA = 0.25;//0.45;//0.6;//0.85;//1.00;
              cout << "DA ON!" << endl;
            }
            else
            {
              DA = 0.0;
            }
          }
          else // turn off Dopamine for non-match
          {
            M1_stim_current = 0;
            DA = 0.0;
            coin_flip = is_match_trial = reward_button_pressed = false;
          }

          DA_debugger << t << " DA: " << DA << " Flip: " << coin_flip << " Current: " << M1_stim_current << " MTS: " << MTS_active 
                      << " Arm Moved: " << arm_moved << " APE-X Button Hit?: " << reward_button_pressed << endl << flush;
        }
      }		
#endif	

#ifdef APEX3
      const int motor_update_interval_ms = 200; //100; //RGM*****
      const int motor_update_offset_ms = 10; //window;
      const int rotation_window = 1000;
      int curr_pose = 0;   
      int num_poses = 4;
#else			
      apex.get_arm_proprioception(shoulder,deltoid,elbow, sdot,ddot,edot, storque, dtorque, etorque, t);
#endif

      sdot=0.0; ddot=0.0; edot=0.0;

      float shoulder_sensory = shoulder;
      float deltoid_sensory = deltoid; 
      float elbow_sensory = elbow;			

      // convert to appropriate cell types from paper on spinal cord.
      const float a=1.0, b=0.28;  // Muscle tendon insertion points (normalized).
      float temp, temp2;
      group g;

      float stim_current = 8000.0;
      float distance = 0.2;
      int interval=pattern_duration_msec;

      if( sec >= M1_stimulation_start_time_sec && sec <= M1_stimulation_end_time_sec && group_data.exists("M1p5(L56)") )
      {
        g = group_data["M1p5(L56)"];

        float stim_current = M1_stim_current; //RGM DEBUG 08/08/2012 . . .M1_stim_current;
        float distance = 0.2;

        float x= g.x0;
        float y= g.y0;
        float width_mm = g.area_width_mm;
        vector<electrode> electrodes(n_patterns);
        
//Expanded pose test: RGM 11/11/10
#define ORIGINAL_STIM_PATTERN
#ifdef ORIGINAL_STIM_PATTERN
        for(kk=0;kk<n_patterns;kk++)
        {					
          electrodes[kk].position[0]=x+xo[kk];//[(n_patterns-1)-kk]; //run patterns backwards to see if maps form early as opposed to forming less for distant arm poses
          electrodes[kk].position[1]=y+yo[kk];//[(n_patterns-1)-kk]; 
          electrodes[kk].position[2]=0;
          electrodes[kk].start_msec=kk*interval;
          electrodes[kk].stop_msec=(kk+1)*interval-50;
          electrodes[kk].dist=distance;
          electrodes[kk].stim_current = stim_current;
        }
#else
        for(kk=0;kk<n_patterns;kk++) 
        {					
          electrodes[kk].position[0] = x + stim_coords[kk][0];//[(n_patterns-1)-kk]; //run patterns backwards to see if maps form early as opposed to forming less for distant arm poses
          electrodes[kk].position[1] = y + stim_coords[kk][1];//[(n_patterns-1)-kk]; 
          electrodes[kk].position[2] = 0;
          electrodes[kk].start_msec = kk*interval;
          electrodes[kk].stop_msec = (kk+1)*interval-50;
          electrodes[kk].dist = distance;
          electrodes[kk].stim_current = stim_current;
        }
#endif		
        stim_group("M1p5(L56)", electrodes, t, trial_duration_msec, exc_output);				
      } // end M1 Stimulation

      /* Part II.  Set the motor commands based on Layer V cell activity. */

      shoulder = 0.0f;
      deltoid = 0.0f; 
      elbow = 0.0f;			

      //***RGM: Place to look at for robot Mental Rotation Test
      g = group_data["M1p5(L56)"];
      gnum=g.last_neuron_id-g.first_neuron_id;

      int window = 10;
      float tau = exp(-1.0/(float)window);  // 100 msec time constant; trying to match time constant of slow twitch muscles.

      shoulder = 0.0f;
      deltoid = 0.0f; 
      elbow = 0.0f;
      float rate_sum = 0.0f;
      // 1. Interpret activity
      int i = g.first_neuron_id;
      int votes = 0;

      for(int row = 0; row< g.rows; row++)
      {
        for( int col = 0; col < g.cols; col++ )
        {
          mfr[i]= tau*mfr[i] + (1.0f-tau)*(spiked[i]?1.0:0.0)*1000 ; // This gives mean rate in Hz.

          if( mfr[i] > 10.0 )
          { // only vote if you're serios about it.
            // Generate population vector which will determine instantaneous posture target.						
            shoulder += mfr[i] * shoulder_lut_rad[row][col];
            deltoid  += mfr[i] * deltoid_lut_rad[row][col];
            elbow    += mfr[i] * elbow_lut_rad[row][col];
            rate_sum += mfr[i];
            votes++;
          }					
          i++;
        }
      }

      if( rate_sum > 0.000001 && votes >= 10 )
      {
        shoulder *= 1.0f/rate_sum;
        deltoid *= 1.0f/rate_sum;
        elbow *= 1.0f/rate_sum;		

        // MTS matched -> move arm
        //shoulder = 0.124802;
        //deltoid = 0.176147;
        //elbow = 0.32725;

        go_home_flag = true;
        match_flag = true;
      }
      else // Go To Home Position
      {
        shoulder = home_pose_radians[0];
        deltoid = home_pose_radians[1];
        elbow = home_pose_radians[2];	

        //shoulder = 0.851359;
        //deltoid = 1.08146;
        //elbow = 0.32725;
        match_flag = false;
      }
      sdot= fabs(shoulder-shoulder_sensory)*max_velocity_rad_s* 0.5;
      ddot= fabs(deltoid-deltoid_sensory)*max_velocity_rad_s* 0.89;
      edot= fabs(elbow-elbow_sensory)*max_velocity_rad_s* 0.5;  // 10% of max velocity.

      //Write Joint Commands and Sensory Data To motorcommands.dat
      if( (t%50) == 0 )
      {
        fout << t << ',' << shoulder << ',' << deltoid << ',' << elbow << ',';
        fout << shoulder_sensory << ',' << deltoid_sensory<< ',' << elbow_sensory << endl << flush;
      }

      MTSp23M_group = group_data["MTSp23M"];

      window = 10;
      tau = exp(-1.0/(float)window);  // 100 msec time constant; trying to match time constant of slow twitch muscles.

      float MTS_rate_sum = 0.0f;
      int MTS_votes = 0;

      for( int i = MTSp23M_group.first_neuron_id; i < MTSp23M_group.last_neuron_id; i++ )
      {
        mfr[i]= tau*mfr[i] + (1.0f-tau)*(spiked[i]?1.0:0.0)*1000 ; //This gives mean rate in Hz.

        if( mfr[i] > 10.0 )
        { 
          MTS_rate_sum += mfr[i];
          MTS_votes++;
        }					
      }	

      //divide out mfr by number of neurons to get*** average mfr for DA
      if( MTS_rate_sum > 0.000001 && MTS_votes >= 10 )
      {
        MTS_active = true;
      }
      else // Go To Home Position
      {
        MTS_active = false;
      }

      /* Arm Moved at all? */
      start_pose = home_pose_radians[0] + home_pose_radians[1];
      current_pose = shoulder + deltoid;
      if( abs(current_pose-start_pose) > movement_threshold )
      {
        arm_moved = true;
      }
      else
      {
        arm_moved = false;
      }

      /* Arm Moved To Target Location? */
      start_pose = target_pose_radians[0] + target_pose_radians[1];
      current_pose = shoulder + deltoid;
      if( abs(current_pose-start_pose) <= target_threshold )
      {
        arm_moved_to_target = true;
      }
      else
      {
        arm_moved_to_target = false;
      }


      //**** 2) Move the arm by writing specified commands. ****
      if( sec >= 0 )
      {
        last_shoulder = shoulder;
        last_deltoid = deltoid;
        last_elbow = elbow;
        last_sdot = sdot;
        last_ddot = ddot;
        last_edot = edot;

        const int motor_update_interval_ms = 250;
        const int motor_update_offset_ms = window;			

#ifdef APEX3
        /* Robot and Rotation Station Control */
        if( myprocid == 0 )
        {
          // Robot Pattern Training Phase 
          if((t < 169000) && (t%training_sample_duration_ms == 900))
          {
            int record_pattern = 0; 
            ++record_counter;

            record_pattern = record_counter%8;
            //J Pattern Sequence Training
            if( record_pattern < 4 )
            {
              apex_client.set_position(SPINNER_ID, spinner_object1, 300, 700); //set rotation
            }
            else
            {
              apex_client.set_position(SPINNER_ID, spinner_object2, 300, 700); //set rotation
            }
            apex_client.set_position(ROTATION_ID,rot_sta_poses[record_pattern], 300, 700); //set rotation 
            apex_client.update_arm_joints(!first_call, home_pose_radians[0], home_pose_radians[1], home_pose_radians[2], 500, 1000, 1200, 750, 750, 750, shoulder, deltoid, elbow, sdot, ddot, edot, storque, dtorque, etorque, t, 50, 0);
          }
          if( t == 170000 )
          {
            record_counter = 0;
          }
          
          // Robot Rotation Training Phase 
          if((t >= 810000 && t < 904000) && (t%training_sample_duration_ms == 900))
          {
            int record_pattern = 0; 
                
            if( t == 855900 )
            {
              record_counter = 0;
            }
            else
            {
              ++record_counter;
            }

            record_pattern = record_counter%4;
            //J Pattern Sequence Training
            if( t >= 810000 && t <= 855000 )
            {
              apex_client.set_position(SPINNER_ID, spinner_object1, 300, 700); //set rotation
            }
            else
            {
              apex_client.set_position(SPINNER_ID, spinner_object2, 300, 700); //set rotation
            }
            apex_client.set_position(ROTATION_ID,rot_sta_poses[record_pattern], 300, 700); //set rotation 
            apex_client.update_arm_joints(!first_call, home_pose_radians[0], home_pose_radians[1], home_pose_radians[2], 500, 1000, 1200, 750, 750, 750, shoulder, deltoid, elbow, sdot, ddot, edot, storque, dtorque, etorque, t, 50, 0);
          }

          //Full Testing & Conditioning Phases
          if( (t >= 170000 && t < 490000) || (t >= 490000 && t < 810000) || (t >= 905000 && t < 1225000 ) )
          {
            //Return Head to home position and Present Pattern A
            if( t%(pattern_trial_duration) == lookat_A_ms )
            {  					
              head_pose = head_pose_home;

              if(t == 170000 || t == 490000 || t == 905000)//905000
              {
                pattern_A = 0;
                pattern_B = 0;
                apex_client.set_position(ROTATION_ID,rot_sta_poses[pattern_A], 300, 700); 
                apex_client.set_position(SPINNER_ID, spinner_object1, 300, 700); 
                cerr << endl << "[1] Set Head(12) to home_position for Pattern_A: " << t << endl << flush;		
                motor_flag = true;
                trial_counter = -1;
                conditioning_cnt = 0;
              }

              match_flag = false;
              first_match = false;
              /*Beginning of trial */
              ++trial_counter;
              std::cout << "Trial count: " << trial_counter << std::endl;
            }
            //Turn Head for Delay and Mental Rotation
            else if( (t%pattern_trial_duration == (lookaway_A_ms + 100)) ) 
            {           
              //set blinder down
              apex_client.set_position(BLINDER_ID, blinder_down, 300, 700);
                      
              cerr << endl << "[2] Set Head(12) to look_away during Delay_A: " << head_pose << " @ " << t << endl << flush;
              motor_flag = true;
            }
            //Present Pattern B while head is turned
            else if(t%pattern_trial_duration == (lookaway_A_ms + 300))
            {
              /* Set Rotation */
              apex_client.set_position(ROTATION_ID, rot_sta_poses[pattern_B], 300, 700);

              //Set-up Spinner for Pattern-B
              if(pattern_B > 3)
              {
                apex_client.set_position(SPINNER_ID, spinner_object2, 306, 700);
              }
              else
              {
                apex_client.set_position(SPINNER_ID, spinner_object1, 306, 700);
              }

              if(t >= 170000 && t < 490000)//conditioning phase
              {
                cout << "******** Conditioning Count: " << conditioning_cnt << " Pattern A: " << pattern_A <<  " Pattern B: " << pattern_B << endl << endl;
                ++conditioning_cnt;
                if(conditioning_cnt%2 == 0)
                {
                  ++pattern_A;
                  if(pattern_A > max_pattern+4)
                  {
                    pattern_A = min_pattern;
                  }
                  pattern_B = pattern_A;
                }
                else
                {
                  //we're odd
                  if(pattern_A > 3)
                  {
                    pattern_B = pattern_A - 4;
                  }
                  else
                  {
                    pattern_B = pattern_A + 4;
                  }
                }
              }
              else
              {
                ++pattern_B;
                if(pattern_B > max_pattern+4)
                {
                  pattern_B = min_pattern;
                  ++pattern_A;     
                  if(pattern_A > max_pattern+4)
                  {
                    pattern_A = min_pattern;
                  }
                }
              }
              cerr << endl << "[3] Set Station(ROTATION_ID) to Pattern_B: " << t << endl << flush;
              motor_flag = true;
            }
            //Turn Head to Home Position to see Pattern B
            else if( t%pattern_trial_duration == (lookat_B_ms - 100) )
            {
              //set blinder up
              apex_client.set_position(BLINDER_ID, blinder_up, 300, 700);

              cerr << endl << "[4] Set Head(12) to home_position for Pattern_B: " << head_pose << " @ " << t << endl << flush;
              motor_flag = true;
            }
            //Set Blinder Down for Response Period
            else if( t%pattern_trial_duration == (lookat_B_ms + 1000) )
            {					
              //set blinder down
              apex_client.set_position(BLINDER_ID, blinder_down, 300, 700);

              motor_flag = true;
            }
            else if( t%pattern_trial_duration == (pattern_trial_duration - 100) )
            { 
              /* Set Rotation Station */
              apex_client.set_position(ROTATION_ID,rot_sta_poses[pattern_A], 300, 700);

              //Set-up Spinner for Pattern-B
              if(pattern_A > 3)
              {
                apex_client.set_position(SPINNER_ID, spinner_object2, 306, 700);
              }
              else
              {
                apex_client.set_position(SPINNER_ID, spinner_object1, 306, 700);
              }

              //set blinder up
              apex_client.set_position(BLINDER_ID, blinder_up, 300, 700);

              cerr << endl << "[6] Set Station(ROTATION_ID) to Pattern_A for next trial: " << t << endl << flush;
              motor_flag = true;
            }
          }		
        } // end if myprocid   

        /* Command Head and/or Rotation Station */
        // Full Testing & Conditioning Phases
        if( (t >= 170000 && t < 490000) || (t >= 490000 && t < 810000) || (t >= 905000 && t < 1225000 ) )
        {
          if(motor_flag)
          {
          cerr << endl << "Call read/write arm method BEFORE matching " << t << endl << flush;
          apex_client.update_arm_joints(!first_call, home_pose_radians[0], home_pose_radians[1], home_pose_radians[2], 500, 1000, 1200, 750, 750, 750, shoulder, deltoid, elbow, sdot, ddot, edot, storque, dtorque, etorque, t, t, 0);
          motor_flag = false;
          }

          if( t%pattern_trial_duration >= lookaway_B_ms && match_flag)
          {       
            if( t%ms_per_sec == 0 )
            {
              //cout << "2) Updating Motor Behavior: t = " << t << " Pattern A = " << pattern_A << " Pattern B = " << pattern_B << endl << endl;
            } 
            if( !first_match )
            {
              cerr << endl << "[5***] Call read/write arm method DURING matching " << t << endl << flush;
              apex_client.update_arm_joints(!first_call, last_shoulder, last_deltoid, last_elbow, 500, 1000, 1200, 750, 750, 750, shoulder, deltoid, elbow, sdot, ddot, edot, storque, dtorque, etorque, t, t, 0);
              first_match = true;
            }
            match_flag = false;
          }
        }
#else
        //Simulated Robot
        //RGM DEBUG 08/08/2012
        //apex.set_arm_command(shoulder, deltoid, elbow, sdot, ddot, edot,t, motor_update_interval_ms, motor_update_offset_ms);
        //cout << "Poses *********** Shoulder= " << shoulder << " Deltoid= " << deltoid << endl << endl << flush;
#endif
      }
    } // if (sec>=0)
    first_call = false;
  } // interact()

  int get_sim_end_time_sec()
  {
    return sim_end_time_sec;
  }

  int sim_end_time_sec;
  ofstream fout, patternhist;
  
#ifdef APEX3
  //apex_robot< typename RobotType >  apex_client;
  apex_robot< ::nsi::thinklink::remote_robot<> >  apex_client;
  int trial_start_ms;
  int trial_duration;
  int lookat_A;
  int lookaway_A;
  int lookat_B;
  int lookaway_B;
#endif

#ifdef APEX2
  robot apex;
#endif

  int max_neuron_id;
  groups group_data;
  int count_fired;
  int myprocid;

  vector<vector<float> > shoulder_lut_rad;
  vector<vector<float> > deltoid_lut_rad;
  vector<vector<float> > elbow_lut_rad;
}; // class world 
